# -*- coding: utf-8 -*-
import colorsys
from abc import ABCMeta, abstractmethod
from copy import deepcopy
from abstractstatic import abstractstatic
import logging
import trimesh
from mayavi import mlab
import numpy as np
from numpy.linalg import inv, norm
import time

from autolab_core import RigidTransform
from .contacts import Contact3D
from .quality import PointGraspMetrics3D
from scipy.linalg import *


def change_ray_color(ray,color):
    colors = np.ones((len(ray.entities), 3))
    colors_1 = (colors * color).astype(np.uint8)  # approach
    ray.colors = colors_1

class Grasp(object):
    """ Abstract grasp class.

    Attributes
    ----------
    configuration : :obj:`numpy.ndarray`
        vector specifying the parameters of the grasp (e.g. hand pose, opening width, joint angles, etc)
    frame : :obj:`str`
        string name of grasp reference frame (defaults to obj)
    """

    __metaclass__ = ABCMeta

    @abstractmethod
    def close_fingers(self, obj, *params):
        """ Finds the contact points by closing on the given object.
        
        Parameters
        ----------
        obj : :obj:`GraspableObject3D`
            object to find contacts on
        *params: other params
        """
        pass

    @abstractmethod
    def configuration(self):
        """ Returns the numpy array representing the hand configuration """
        pass

    @abstractmethod
    def frame(self):
        """ Returns the string name of the grasp reference frame  """
        pass

    @abstractstatic
    def params_from_configuration(configuration):
        """ Convert configuration vector to a set of params for the class """
        pass

    @abstractstatic
    def configuration_from_params(*params):
        """ Convert param list to a configuration vector for the class """
        pass


class PointGrasp(Grasp):
    """ Abstract grasp class for grasps with a point contact model.

    Attributes
    ----------
    configuration : :obj:`numpy.ndarray`
        vector specifying the parameters of the grasp (e.g. hand pose, opening width, joint angles, etc)
    frame : :obj:`str`
        string name of grasp reference frame (defaults to obj)
    """
    __metaclass__ = ABCMeta

    @abstractmethod
    def create_line_of_action(g, axis, width, obj, num_samples):
        """ Creates a line of action, or the points in space that the grasp traces out, from a point g in world coordinates on an object.

        Returns
        -------
        bool
            whether or not successful
        :obj:`list` of :obj:`numpy.ndarray`
            points in 3D space along the line of action
        """
        pass


class ParallelJawPtGrasp3D(PointGrasp):
    """ Parallel Jaw point grasps in 3D space.
    """

    def __init__(self, configuration, frame='object', grasp_id=None):
        # get parameters from configuration array
        grasp_center, grasp_axis, grasp_width, grasp_angle, jaw_width, grasp_score, min_grasp_width = \
            ParallelJawPtGrasp3D.params_from_configuration(configuration)
        self.approach_angle_ = grasp_angle

        self.center_ = grasp_center
        self.axis_ = grasp_axis / np.linalg.norm(grasp_axis)
        self.max_grasp_width_ = grasp_width
        self.jaw_width_ = jaw_width
        self.real_jaw_width_ = self.jaw_width_
        self.min_grasp_width_ = min_grasp_width
        self.grasp_score= grasp_score
        self.frame_ = frame
        self.grasp_id_ = grasp_id

    @property
    def center(self):
        """ :obj:`numpy.ndarray` : 3-vector specifying the center of the jaws """
        return self.center_

    @center.setter
    def center(self, x):
        self.center_ = x

    @property
    def axis(self):
        """ :obj:`numpy.ndarray` : normalized 3-vector specifying the line between the jaws """
        return self.axis_

    @property
    def open_width(self):
        """ float : maximum opening width of the jaws """
        return self.max_grasp_width_

    @property
    def close_width(self):
        """ float : minimum opening width of the jaws """
        return self.min_grasp_width_

    @property
    def jaw_width(self):
        """ float : width of the jaws in the tangent plane to the grasp axis """
        return self.jaw_width_

    @property
    def real_jaw_width(self):
        """ float : width of the jaws in the tangent plane to the grasp axis """
        return self.real_jaw_width_

    @property
    def approach_angle(self):
        """ float : approach angle of the grasp """
        return self.approach_angle_

    @property
    def configuration(self):
        return ParallelJawPtGrasp3D.configuration_from_params(self.center_, self.axis_, self.max_grasp_width_,
                                                              self.approach_angle_, self.jaw_width_, self.grasp_score,
                                                              self.min_grasp_width_)

    @property
    def frame(self):
        """ :obj:`str` : name of grasp reference frame """
        return self.frame_

    @property
    def id(self):
        """ int : id of grasp """
        return self.grasp_id_

    @frame.setter
    def frame(self, f):
        self.frame_ = f

    @approach_angle.setter
    def approach_angle(self, angle):
        """ Set the grasp approach angle """
        self.approach_angle_ = angle

    @property
    def endpoints(self):
        """
        Returns
        -------
        :obj:`numpy.ndarray`
            location of jaws in 3D space at max opening width """
        return self.center_ - (self.max_grasp_width_ / 2.0) * self.axis_, self.center_ + (self.max_grasp_width_ / 2.0) * self.axis_,

    @property
    def contactpoints(self):
        return self.center_ - (self.jaw_width_ / 2.0) * self.axis_, self.center_ + (self.jaw_width_ / 2.0) * self.axis_

    @property
    def virtual_contactpoints(self):
        return self.center_ - (self.real_jaw_width_ / 2.0) * self.axis_, self.center_ + (self.real_jaw_width_ / 2.0) * self.axis_

    @staticmethod
    def distance(g1, g2, alpha=0.06, beta=0.01):
        """ Evaluates the distance between two grasps.

        Parameters
        ----------
        g1 : :obj:`ParallelJawPtGrasp3D`
            the first grasp to use
        g2 : :obj:`ParallelJawPtGrasp3D`
            the second grasp to use
        alpha : float
            parameter weighting rotational versus spatial distance

        Returns
        -------
        float
            distance between grasps g1 and g2
        """
        center_dist = np.linalg.norm(g1.center - g2.center)
        value = np.abs(g1.axis.dot(g2.axis))
        value_1 = g1.rotated_full_axis[:,0].dot(g2.rotated_full_axis[:,0])
        if value > 1:
            value = 1
        if value_1 > 1:
            value_1 = 1
        if value_1 < -1:
            value_1 = -1

        axis_dist = (1.0 / np.pi) * np.arccos(value)
        approach_dist = (1.0 / np.pi) * np.arccos(value_1) 
        #approach_dist = 0
        return center_dist + alpha * axis_dist + beta * approach_dist

    @staticmethod
    def distance_L2(point1, point2, grasp_c1_axis_world):

        dist = np.dot(grasp_c1_axis_world, (point1- point2))
        return dist

    @staticmethod
    def configuration_from_params(center, axis, width, angle=0, jaw_width=0, grasp_score=[0., 0., 0.], min_width=0):
        """ Converts grasp parameters to a configuration vector. """
        if np.abs(np.linalg.norm(axis) - 1.0) > 1e-5:
            raise ValueError('Illegal grasp axis. Must be norm one')
        configuration = np.zeros(13)
        configuration[0:3] = center
        configuration[3:6] = axis
        configuration[6] = width
        configuration[7] = angle
        configuration[8] = jaw_width
        configuration[9:12] = grasp_score
        configuration[12] = min_width
        return configuration


    @staticmethod
    def show_line(ori, target, color='g', scale_factor=0.0002):
        if color == 'b':
            color_f = (0, 0, 1)
        elif color == 'r':
            color_f = (1, 0, 0)
        elif color == 'g':
            color_f = (0, 1, 0)
        else:
            color_f = (1, 1, 1)
        mlab.plot3d([ori[0], target[0]], [ori[1], target[1]], [ori[2], target[2]], color=color_f, tube_radius=scale_factor)

    @staticmethod
    def params_from_configuration(configuration):
        """ Converts configuration vector into grasp parameters.
        
        Returns
        -------
        grasp_center : :obj:`numpy.ndarray`
            center of grasp in 3D space
        grasp_axis : :obj:`numpy.ndarray`
            normalized axis of grasp in 3D space
        max_width : float
            maximum opening width of jaws
        angle : float
            approach angle
        jaw_width : float
            width of jaws
        min_width : float
            minimum closing width of jaws
        """
        if not isinstance(configuration, np.ndarray) or (configuration.shape[0] != 13):
            raise ValueError('Configuration must be numpy ndarray of size 13')
        min_grasp_width = configuration[12]
        if np.abs(np.linalg.norm(configuration[3:6]) - 1.0) > 1e-5:
            raise ValueError('Illegal grasp axis. Must be norm one')
        return configuration[0:3], configuration[3:6], configuration[6], configuration[7], configuration[8], configuration[9:12], min_grasp_width

    @staticmethod
    def center_from_endpoints(g1, g2):
        """ Grasp center from endpoints as np 3-arrays """
        grasp_center = (g1 + g2) / 2
        return grasp_center

    @staticmethod
    def axis_from_endpoints(g1, g2):
        """ Normalized axis of grasp from endpoints as np 3-arrays """
        grasp_axis = g2 - g1
        if np.linalg.norm(grasp_axis) == 0:
            return grasp_axis
        return grasp_axis / np.linalg.norm(grasp_axis)

    @staticmethod
    def width_from_endpoints(g1, g2):
        """ Width of grasp from endpoints """
        grasp_axis = g2 - g1
        return np.linalg.norm(grasp_axis)

    @property
    def unrotated_full_axis(self):
        """ Rotation matrix from canonical grasp reference frame to object reference frame. X axis points out of the gripper palm along the 0-degree approach direction, Y axis points between the jaws, and the Z axs is orthogonal.

        Returns
        -------
        :obj:`numpy.ndarray`
            rotation matrix of grasp
        """
        grasp_axis_y = self.axis
        grasp_axis_x = np.array([grasp_axis_y[1], -grasp_axis_y[0], 0])
        if np.linalg.norm(grasp_axis_x) == 0:
            grasp_axis_x = np.array([1,0,0])
        grasp_axis_x = grasp_axis_x / norm(grasp_axis_x)
        grasp_axis_z = np.cross(grasp_axis_x, grasp_axis_y)
       
        R = np.c_[grasp_axis_x, np.c_[grasp_axis_y, grasp_axis_z]]
        return R

    @property
    def rotated_full_axis(self):
        """ Rotation matrix from canonical grasp reference frame to object reference frame. X axis points out of the gripper palm along the grasp approach angle, Y axis points between the jaws, and the Z axs is orthogonal.

        Returns
        -------
        :obj:`numpy.ndarray`
            rotation matrix of grasp
        """
        R = ParallelJawPtGrasp3D._get_rotation_matrix_y(self.approach_angle)
        R = self.unrotated_full_axis.dot(R)
        return R

    @property
    def T_grasp_obj(self):
        """ Rigid transformation from grasp frame to object frame.
        Rotation matrix is X-axis along approach direction, Y axis pointing between the jaws, and Z-axis orthogonal.
        Translation vector is the grasp center.

        Returns
        -------
        :obj:`RigidTransform`
            transformation from grasp to object coordinates
        """

        T_grasp_obj = RigidTransform(self.rotated_full_axis, self.center, from_frame='grasp', to_frame='obj')
        return T_grasp_obj

    @property
    def T_finger_obj(self):
        """ Rigid transformation from grasp frame to object frame.
        Rotation matrix is X-axis along approach direction, Y axis pointing between the jaws, and Z-axis orthogonal.
        Translation vector is the grasp center.

        Returns
        -------
        :obj:`RigidTransform`
            transformation from grasp to object coordinates
        """

        T_finger_left_obj = RigidTransform(self.rotated_full_axis, self.virtual_contactpoints[0],
                                           from_frame='left_finger', to_frame='obj')
        T_finger_right_obj = RigidTransform(self.rotated_full_axis, self.virtual_contactpoints[1],
                                            from_frame='right_finger', to_frame='obj')
        return T_finger_left_obj, T_finger_right_obj

    @staticmethod
    def _get_rotation_matrix_y(theta):
        theta = theta / 180 * np.pi
        cos_t = np.cos(theta)
        sin_t = np.sin(theta)
        R = np.c_[[cos_t, 0, sin_t], np.c_[[0, 1, 0], [-sin_t, 0, cos_t]]]
        return R

    def gripper_pose(self, gripper=None):
        """ Returns the RigidTransformation from the gripper frame to the object frame when the gripper is executing the given grasp.
        Differs from the grasp reference frame because different robots use different conventions for the gripper reference frame.
        
        Parameters
        ----------
        gripper : :obj:`RobotGripper`
            gripper to get the pose for

        Returns
        -------
        :obj:`RigidTransform`
            transformation from gripper frame to object frame
        """
        if gripper is None:
            T_gripper_grasp = RigidTransform(from_frame='gripper', to_frame='grasp')
        else:
            T_gripper_grasp = gripper.T_grasp_gripper            

        T_gripper_obj = self.T_grasp_obj * T_gripper_grasp
        return T_gripper_obj

    def close_fingers(self, obj, gripper, collision_manager, vis=False, check_approach=True, check_closing=True,
                      approach_dist=0.2):
        """ Steps along grasp axis to find the locations of contact with an object

        Parameters
        ----------
        obj : :obj:`GraspableObject3D`
            object to close fingers on
        vis : bool
            whether or not to plot the line of action and contact points
        check_approach : bool
            whether or not to check if the contact points can be reached
        approach_dist : float
            how far back to check the approach distance, only if checking the approach is set
        
        Returns
        -------
        success : bool
            whether or not contacts were found
        c1 : :obj:`Contact3D`
            the contact point for jaw 1
        c2 : :obj:`Contact3D`
            the contact point for jaw 2
        """
        # get grasp endpoints in sdf frame
        g1_world, g2_world = self.contactpoints
        #print('g1_world is : {} g2_world is : {}'.format(g1_world, g2_world))

        # check for contact along approach
        if check_approach:
            approach_axis = self.rotated_full_axis[:, 0]
            z_axis = self.rotated_full_axis[:, 2]
            directions = [-approach_axis] * 14

            width_ = gripper.max_width/2 - gripper.pad - self.jaw_width_ / 2
            k = int(width_ // 0.001 + 1)
            k = k if k < 7 else 7
            for i in range(k):
                ray_origins = [g1_world-self.axis_*(i+0.3)*1e-3,
                        g1_world-self.axis_*(i+0.3)*1e-3+z_axis*0.009, g1_world-self.axis_*(i+0.3)*1e-3-z_axis*0.009,
                        g1_world-self.axis_*(i+0.3)*1e-3+z_axis*0.006, g1_world-self.axis_*(i+0.3)*1e-3-z_axis*0.006,
                        g1_world-self.axis_*(i+0.3)*1e-3+z_axis*0.003, g1_world-self.axis_*(i+0.3)*1e-3-z_axis*0.003,
                        g2_world+self.axis_*(i+0.3)*1e-3,
                        g2_world+self.axis_*(i+0.3)*1e-3+z_axis*0.009, g2_world+self.axis_*(i+0.3)*1e-3-z_axis*0.009,
                        g2_world+self.axis_*(i+0.3)*1e-3+z_axis*0.006, g2_world+self.axis_*(i+0.3)*1e-3-z_axis*0.006,
                        g2_world+self.axis_*(i+0.3)*1e-3+z_axis*0.003, g2_world+self.axis_*(i+0.3)*1e-3-z_axis*0.003]

                locations, _, _ = ParallelJawPtGrasp3D.find_contact(obj, ray_origins=ray_origins,
                                                                    ray_directions=directions, vis=vis)

                if vis:
                    ray_visualize = trimesh.load_path(np.hstack((
                        np.asarray(ray_origins),
                        np.asarray(ray_origins) + np.asarray(directions))).reshape(-1, 2, 3))
                    pc_ori = trimesh.PointCloud(ray_origins, colors=[0,255,0])
                    scene_ray = trimesh.Scene([ray_visualize, pc_ori, obj.mesh])
                    scene_ray.show()
                approach_collision = len(locations) > 0
                closing_collision = True
                if not approach_collision:
                    # self.center_ = center + (((i+right_gain) - (i+left_gain)) / 2) * self.axis_ * 0.001
                    self.real_jaw_width_ = self.jaw_width_ + (2*i)*1e-3
                    if check_closing:
                        finger_left = deepcopy(gripper.finger_mesh)
                        base = deepcopy(gripper.base_mesh)
                        finger_right = deepcopy(finger_left)
                        T_base = trimesh.transformations.translation_matrix([0, 0, -0.13])
                        R_base = trimesh.transformations.euler_matrix(np.pi/2, np.pi/2, 0, 'rxyz')
                        base.apply_transform(T_base)
                        base.apply_transform(R_base)
                        base.apply_transform(self.T_grasp_obj.matrix)
                        T_leftfinger, T_rightfinger = self.T_finger_obj

                        T = trimesh.transformations.translation_matrix([-(gripper.pad), -0.0065, -0.048])
                        finger_left.apply_transform(T)
                        finger_right.apply_transform(T)
                        R_l = trimesh.transformations.euler_matrix(np.pi/2, np.pi/2, 0, 'rxyz')
                        R_r = trimesh.transformations.euler_matrix(np.pi/2, np.pi/2, np.pi, 'rxyz')
                        
                        finger_left.apply_transform(R_l)
                        finger_right.apply_transform(R_r)
                        finger_left.apply_transform(T_leftfinger.matrix)
                        finger_right.apply_transform(T_rightfinger.matrix)
                        # vis = True
                        if vis:
                            scene_tmp = trimesh.Scene([finger_left, finger_right, base, obj.mesh])
                            scene_tmp.show()

                        is_collision_left = collision_manager.in_collision_single(finger_left, return_names=False,
                                                                                  return_data=False)

                        if is_collision_left:

                            # print('approach_closing collision left')
                            continue

                        is_collision_right = collision_manager.in_collision_single(finger_right, return_names=False,
                                                                                   return_data=False)

                        if is_collision_right:

                            # print('approach_closing collision right')
                            continue

                        is_collision_base = collision_manager.in_collision_single(base, return_names=False,
                                                                                  return_data=False)
                        if is_collision_base:
                            # print('approach_closing collision base')
                            continue
                        closing_collision =False
                        break
    
            if approach_collision or closing_collision:
                #print('approach collision')
                return False
        # vis = True
        # print('show here', self.center_)

        if vis:
            ori =self.contactpoints[0] + self.axis/10
            target = self.contactpoints[0] - self.axis/10
            ParallelJawPtGrasp3D.show_line(ori, target, scale_factor=0.0001)
            #add by lifuyu
            mlab.points3d(self.center_[0],self.center_[1],self.center_[2],scale_factor=0.001)
            #--------------------------------
            mlab.points3d(self.virtual_contactpoints[0][0], self.virtual_contactpoints[0][1], self.virtual_contactpoints[0][2], scale_factor=0.001)
            mlab.points3d(self.virtual_contactpoints[1][0], self.virtual_contactpoints[1][1], self.virtual_contactpoints[1][2], scale_factor=0.001)
            mlab.triangular_mesh(obj.mesh.vertices[:, 0], obj.mesh.vertices[:, 1], obj.mesh.vertices[:, 2], obj.mesh.faces, representation='surface', color=(0.5, 0.5, 0), opacity=1.0)
            mlab.triangular_mesh(finger_left.vertices[:, 0], finger_left.vertices[:, 1], finger_left.vertices[:, 2], finger_left.faces, representation='surface', color=(0, 0.5, 0.5), opacity=1.0)
            mlab.triangular_mesh(finger_right.vertices[:, 0], finger_right.vertices[:, 1], finger_right.vertices[:, 2], finger_right.faces, representation='surface', color=(0, 0.5, 0.5), opacity=1.0)

            mlab.show()
            pass

        contacts_found = True
        return contacts_found

    @staticmethod
    def create_line_of_action(g, axis, width, obj, num_samples, min_width = 0, convert_grid=True):
        """
        Creates a straight line of action, or list of grid points, from a given point and direction in world or grid coords

        Parameters
        ----------
        g : 3x1 :obj:`numpy.ndarray`
            start point to create the line of action
        axis : normalized 3x1 :obj:`numpy.ndarray`
            normalized numpy 3 array of grasp direction
        width : float
            the grasp width
        num_samples : int
            number of discrete points along the line of action
        convert_grid : bool
            whether or not the points are specified in world coords

        Returns
        -------
        line_of_action : :obj:`list` of 3x1 :obj:`numpy.ndarrays`
            coordinates to pass through in 3D space for contact checking
        """
        num_samples = max(num_samples, 3) # always at least 3 samples
        line_of_action = [g + t * axis for t in np.linspace(0, float(width) / 2 - float(min_width) / 2, num = num_samples)]
        if convert_grid:
            as_array = np.array(line_of_action).T
            transformed = obj.sdf.transform_pt_obj_to_grid(as_array)
            line_of_action = list(transformed.T)
        return line_of_action
        
    @staticmethod
    def find_contact(obj, ray_origins, ray_directions, vis=True):
        locations, index_ray, index_tri = obj.mesh.ray.intersects_location(ray_origins, ray_directions, multiple_hits=True)
        return locations, index_ray, index_tri

    @staticmethod
    def grasp_from_contact_and_axis_on_mesh(gripper, obj, c1, grasp_axis_world, grasp_width_world, grasp_angle=0,
                                            friction_coef=None, vis=False):
        """
        Creates a grasp from a single contact point in grid coordinates and direction in grid coordinates.
        
        Parameters
        ----------
        obj : :obj:`GraspableObject3D`
            object to create grasp for
        grasp_c1_grid : 3x1 :obj:`numpy.ndarray`
            contact point 1 in world
        grasp_axis : normalized 3x1 :obj:`numpy.ndarray`
           normalized direction of the grasp in world
        grasp_width_world : float
            grasp_width in world coords
        jaw_width_world : float
            width of jaws in world coords
        min_grasp_width_world : float
            min closing width of jaws
        vis : bool
            whether or not to visualize the grasp
        
        Returns
        -------
        g : :obj:`ParallelJawGrasp3D`
            grasp created by finding the second contact
        c1 : :obj:`Contact3D`
            first contact point on the object
        c2 : :obj:`Contact3D`
            second contact point on the object
        """
        # transform to grid basis
        assert friction_coef is not None
        grasp_c1_world = c1.point
        grasp_axis_world = grasp_axis_world / np.linalg.norm(grasp_axis_world)
        # compute the contact points on the object
        point_ori = deepcopy(grasp_c1_world)
        point_ori -= (gripper.pad+gripper.finger_tip_width) * grasp_axis_world
        c, _, index_tri = ParallelJawPtGrasp3D.find_contact(obj, ray_origins=point_ori[np.newaxis, :], ray_directions=grasp_axis_world[np.newaxis, :], vis=vis)
        if vis:
            if len(c) > 1 and vis:
                fig = mlab.figure(size=(400, 400))
                mlab.points3d(grasp_c1_world[0],grasp_c1_world[1], grasp_c1_world[2], color=(1, 0, 0), scale_factor=0.001)
                for point in c:
                    mlab.points3d(point[0], point[1], point[2], color=(0, 0, 1), scale_factor=0.0008)

                mlab.triangular_mesh(obj.mesh.vertices[:, 0], obj.mesh.vertices[:, 1], obj.mesh.vertices[:, 2], obj.mesh.faces, representation='surface', color=(0.5, 0.5, 0), opacity=1.0)

                ori = grasp_c1_world + grasp_axis_world/10
                target = grasp_c1_world - grasp_axis_world * (gripper.pad+gripper.finger_tip_width)
                ParallelJawPtGrasp3D.show_line(ori, target, scale_factor=0.0001)
                
                mlab.show()
        c2 = []
        grasp = []
        distance_pre = 1
        data_ = zip(c, index_tri)
        data = sorted(data_, key=lambda item: ParallelJawPtGrasp3D.distance_L2(item[0], grasp_c1_world, grasp_axis_world), reverse=True)
        contact_found = False
        if c is None:
            contact_found = False
        else:
            for point, idx in data:
                distance_ = ParallelJawPtGrasp3D.distance_L2(point, grasp_c1_world, grasp_axis_world)
                if distance_ < -0.001:
                    #print('found obstacle in oppsite direction')
                    contact_found = False
                    break

                if gripper.max_width - 2 * gripper.pad > distance_ > gripper.min_width:
                    if obj.badpoints_mask[obj.mesh.faces[idx][0]] and obj.badpoints_mask[obj.mesh.faces[idx][1]] and obj.badpoints_mask[obj.mesh.faces[idx][2]]:
                        c2_tmp = Contact3D(obj, index=idx, contact_point=point)
                        force_closure_score = PointGraspMetrics3D.force_closure(c1, c2_tmp, friction_coef)
                        if force_closure_score > 0:
                            contact_found = True
                            if distance_pre - distance_ > gripper.pad + gripper.finger_tip_width:
                                #vis = True
                                if vis == True:
                                    fig = mlab.figure(size=(400, 400))
                                    mlab.points3d(grasp_c1_world[0],grasp_c1_world[1], grasp_c1_world[2], color=(1, 0, 0), scale_factor=0.0008)
                                    mlab.points3d(point[0], point[1], point[2], color=(0, 0, 1), scale_factor=0.0008)
                                    mlab.triangular_mesh(obj.mesh.vertices[:, 0], obj.mesh.vertices[:, 1], obj.mesh.vertices[:, 2], obj.mesh.faces, representation='surface', color=(0.5, 0.5, 0), opacity=1.0)
                                    ori = grasp_c1_world + grasp_axis_world/10
                                    target = grasp_c1_world - grasp_axis_world * (gripper.pad+gripper.finger_tip_width)
                                    ParallelJawPtGrasp3D.show_line(ori, target, scale_factor=0.0001)
                                    
                                    mlab.show()

                                grasp_center = ParallelJawPtGrasp3D.center_from_endpoints(grasp_c1_world, c2_tmp.point)
                                grasp_axis = grasp_axis_world
                                jaw_width_world = distance_
                                grasp_force_closure_score = force_closure_score * 0.50
                                value = 1 - np.linalg.norm(grasp_center) / 0.03
                                value = value if value > -3 else -3
                                grasp_center_score = value * 0.20
                                #grasp_width_score = (jaw_width_world // ((gripper.max_width - 2 * gripper.pad)/4) + 1)/4 * 0.15
                                grasp_curvature_score = c1.curvature_score * c2_tmp.curvature_score * 0.30
                                grasp_score_sum =grasp_force_closure_score + grasp_center_score + grasp_curvature_score
                                grasp_score = [grasp_force_closure_score, grasp_center_score, grasp_curvature_score]
                                if grasp_score_sum <= 0.0:
                                    continue
                                configuration = ParallelJawPtGrasp3D.configuration_from_params(grasp_center, grasp_axis,
                                                                                               grasp_width_world,
                                                                                               grasp_angle,
                                                                                               jaw_width_world,
                                                                                               grasp_score)
                                grasp.append(ParallelJawPtGrasp3D(configuration))
                                c2.append(c2_tmp)
                        else:
                            #print('do not meet force_closure property')
                            pass
                    else:
                        #print('do not meet badpoint property')
                        pass
                else:
                    #print('do not meet distance property')
                    pass
                distance_pre = distance_
        if not contact_found:
            logging.debug('No contacts found for grasp')
            return False, [], []
        return contact_found, grasp, c2 # relative to object

